#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Hubs,  S2, HTMotor,  none,     none,     none)
#pragma config(Hubs,  S3, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Sensor, S3,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     DriveFL,       tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     DriveBL,       tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     LiftL,         tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C1_1,     DriveFR,       tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S2_C1_2,     DriveBR,       tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S3_C1_1,     LiftR,         tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S3_C1_2,     Roller,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S3_C2_1,    TubeG,                tServoStandard)
#pragma config(Servo,  srvo_S3_C2_2,    LDoor,                tServoStandard)
#pragma config(Servo,  srvo_S3_C2_3,    RDoor,                tServoStandard)
#pragma config(Servo,  srvo_S3_C2_4,    SensorServo,          tServoStandard)
#pragma config(Servo,  srvo_S3_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S3_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"

// testing

//task lift()

 //while(nMotorEncoder[LiftR] < 1675)
  // {
  // 	motor[LiftR] = 100;
	//	motor[LiftL] = 100;

	//	}

	//	motor[LiftR] = 15;
	//	motor[LiftL] = 15;

  // servo[TubeG] = 255;
  // stoptask lift()

task main()
{

nMotorEncoder[LiftR] = 0;
nMotorEncoder[LiftL] = 0;

nMotorEncoder[DriveFR] = 0;
nMotorEncoder[DriveFL] = 0;

servo[LDoor] = 0;
servo[RDoor] = 256;
servo[TubeG] = 186;
servo[SensorServo] = 20;

waitForStart();

while (nMotorEncoder [DriveFR] > (-3 * 101) - 4)

	{

	motor[DriveFR] = -30;
	motor[DriveFL] = -30;
	motor[DriveBR] = -30;
	motor[DriveBL] = -30;


}

while (nMotorEncoder [DriveFR] > (-85 * 101) - 4)

	{

	motor[DriveFR] = -100;
	motor[DriveFL] = -100;
	motor[DriveBR] = -100;
	motor[DriveBL] = -100;


}



	motor[DriveFR] = 0;
	motor[DriveFL] = 0;
	motor[DriveBR] = 0;
	motor[DriveBL] = 0;

nMotorEncoder[DriveFR] = 0;


		while(nMotorEncoder[LiftR] < 1675)
		{

		motor[LiftR] = 100;
		motor[LiftL] = 100;

		}

		motor[LiftR] = 15;
		motor[LiftL] = 15;


		servo[TubeG] = 255;


while (nMotorEncoder [DriveFR] > (-10 * 101 - 140))

	{

	motor[DriveFR] = -25;
	motor[DriveFL] = -25;
	motor[DriveBR] = -25;
	motor[DriveBL] = -25;

	}

	motor[DriveFR] = 0;
	motor[DriveFL] = 0;
	motor[DriveBR] = 0;
	motor[DriveBL] = 0;

nMotorEncoder[DriveFR] = 0;

wait1Msec(250);

servo[RDoor] = 172;

wait1Msec(1100);

servo[TubeG] = 186;

while (nMotorEncoder[LiftR] > 800)

			{

					motor[DriveFR] = 0;
					motor[DriveFL] = 0;
					motor[DriveBR] = 0;
					motor[DriveBL] = 0;


					motor[LiftR] = 1;
					motor[LiftL] = 1;

			}

		while(nMotorEncoder[LiftR] >= 50)

			{

			motor[DriveFR] = 0;
			motor[DriveFL] = 0;
			motor[DriveBR] = 0;
			motor[DriveBL] = 0;


			motor[LiftR] = -1;
			motor[LiftL] = -1;

			}

		motor[LiftR] = 0;
		motor[LiftL] = 0;

		nMotorEncoder[LiftR] = 0;
		nMotorEncoder[LiftL] = 0;

nMotorEncoder[DriveFR] = 0;
nMotorEncoder[DriveFL] = 0;


while (nMotorEncoder [DriveFR] > -750)

	{

	motor[DriveFR] = -100;
	motor[DriveBR] = -100;

	motor[DriveFL] = -100;
	motor[DriveBL] = -100;

	}

	motor[DriveFR] = 0;
	motor[DriveFL] = 0;
	motor[DriveBR] = 0;
	motor[DriveBL] = 0;

nMotorEncoder[DriveFR] = 0;
nMotorEncoder[DriveFL] = 0;



while (nMotorEncoder [DriveFR] < 850)

	{

	motor[DriveFR] = 100;
	motor[DriveBR] = 100;

	motor[DriveFL] = 100;
	motor[DriveBL] = 100;

	}

	motor[DriveFR] = 0;
	motor[DriveFL] = 0;
	motor[DriveBR] = 0;
	motor[DriveBL] = 0;

nMotorEncoder[DriveFR] = 0;
nMotorEncoder[DriveFL] = 0;

wait1Msec(500);


while (nMotorEncoder [DriveFR] < 3800)
	{

	motor[DriveFR] = 70;
	motor[DriveBR] = 70;

	motor[DriveFL] = -70;
	motor[DriveBL] = -70;

	}

	motor[DriveFR] = 0;
	motor[DriveFL] = 0;
	motor[DriveBR] = 0;
	motor[DriveBL] = 0;

nMotorEncoder[DriveFR] = 0;
nMotorEncoder[DriveFL] = 0;


while (nMotorEncoder [DriveFR] > (-87 * 101) - 4)

	{

	motor[DriveFR] = -15;
	motor[DriveFL] = -100;
	motor[DriveBR] = -15;
	motor[DriveBL] = -100;

	}

	motor[DriveFR] = 0;
	motor[DriveFL] = 0;
	motor[DriveBR] = 0;
	motor[DriveBL] = 0;

nMotorEncoder[LiftR] = 0;

nMotorEncoder[DriveFR] = 0;

while (nMotorEncoder [DriveFR] < 50)
	{

	motor[DriveFR] = 30;
	motor[DriveBR] = 30;

	motor[DriveFL] = -100;

	motor[DriveBL] = -100;
	}

	motor[DriveFR] = 0;
	motor[DriveFL] = 0;
	motor[DriveBR] = 0;
	motor[DriveBL] = 0;


	while(nMotorEncoder[LiftR] < 500)
		{

		motor[LiftR] = 100;
		motor[LiftL] = 100;

		}

		motor[LiftR] = 15;
		motor[LiftL] = 15;


	servo[TubeG] = 255;

	servo[RDoor] = 256;


while (nMotorEncoder [DriveFR] > -300)
	{

	motor[DriveFR] = -100;
	motor[DriveBR] = -100;

	motor[DriveFL] = 100;

	motor[DriveBL] = 100;
	}

	motor[DriveFR] = 0;
	motor[DriveFL] = 0;
	motor[DriveBR] = 0;
	motor[DriveBL] = 0;

nMotorEncoder[DriveFL] = 0;

nMotorEncoder[DriveFR] = 0;

while (nMotorEncoder [DriveFR] < (85 * 101) + 4)

	{

	motor[DriveFR] = 100;
	motor[DriveFL] = 100;
	motor[DriveBR] = 100;
	motor[DriveBL] = 100;

	}


	motor[DriveFR] = 30;
	motor[DriveFL] = 30;
	motor[DriveBR] = 30;
	motor[DriveBL] = 30;

	wait1Msec(1000);

	motor[DriveFR] = 0;
	motor[DriveFL] = 0;
	motor[DriveBR] = 0;
	motor[DriveBL] = 0;

nMotorEncoder[DriveFL] = 0;

while (nMotorEncoder [DriveFL] < 4100)

	{

	motor[DriveFR] = -70;
	motor[DriveBR] = -70;

	motor[DriveFL] = 70;
	motor[DriveBL] = 70;

	}

	motor[DriveFR] = 0;
	motor[DriveFL] = 0;
	motor[DriveBR] = 0;
	motor[DriveBL] = 0;

nMotorEncoder[DriveFR] = 0;
nMotorEncoder[DriveFL] = 0;



while (nMotorEncoder [DriveFR] > -2000)

	{

	motor[DriveFR] = -100;
	motor[DriveBR] = -100;

	motor[DriveFL] = -100;
	motor[DriveBL] = -100;

	}

	motor[DriveFR] = 0;
	motor[DriveFL] = 0;
	motor[DriveBR] = 0;
	motor[DriveBL] = 0;

nMotorEncoder[DriveFR] = 0;
nMotorEncoder[DriveFL] = 0;



while(nMotorEncoder[LiftR] < 2600)
		{

		motor[LiftR] = 100;
		motor[LiftL] = 100;

		}

		motor[LiftR] = 15;
		motor[LiftL] = 15;

servo[LDoor] = 65;

wait1Msec(350);

	motor[DriveFR] = -100;
	motor[DriveBR] = -100;

	motor[DriveFL] = -100;
	motor[DriveBL] = -100;


wait1Msec(650);


	motor[DriveFR] = 0;
	motor[DriveBR] = 0;

	motor[DriveFL] = 0;
	motor[DriveBL] = 0;

servo[LDoor] = 0;

servo[TubeG] = 186;

wait1Msec(1500);

}
